#ifndef MATH_HPP
#define MATH_HPP

////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include <iostream>
#include <cmath>
#include <algorithm>

#include <SFML/System/Vector2.hpp>
#include <SFML/System/Vector3.hpp>
#include <SFML/Graphics/Transform.hpp>

#include <SmegTools/Config.h>

#define ZERO_EPSILON 0.001f

namespace smeg {
    namespace math {

// Some Consts
const float PI                  = 3.1415926535897932f;
const float HALF_PI             = PI * 0.5f;
const float TWO_PI              = PI * 2.0f;
const float THREE_PI_OVER_TWO   = PI * 1.5f;
const float RAD_TO_DEG          = 57.295779513082321600f;            // 180 / Pi
const float DEG_TO_RAD          = 0.017453292519943296f;             // Pi / 180

const float SQRT2               = 1.4142135623730951f;

////////////////////////////////////////////////////////////
//  some random number functions.
////////////////////////////////////////////////////////////
#define RAND_MAX 0x7fff // Maximum value return by rand()

//returns a random integer between x and y
inline int RandRange(int x,int y)
{
  return x + (rand() % ( y - x + 1 ));
}

//returns a random float between zero and 1
inline float RandFloat() {
    return (rand() / (RAND_MAX + 1.0f));
}

inline float RandRange(float x, float y)
{
  return x + RandFloat() * ( y - x );
}

//returns a random double in the range -1 < n < 1
inline float RandClamped() {
    return RandFloat() - RandFloat();
}

////////////////////////////////////////////////////////////
/// Get a valid degree angle between 0 and 360 degrees
///
/// \param _fAngleDeg : Input angle
///
/// \return A valid angle between 0 and 360 degrees
///
////////////////////////////////////////////////////////////
float GetValidDegreeAngle( float _fAngleDeg );

////////////////////////////////////////////////////////////
/// Get a valid radian angle between 0 and 2PI
///
/// \param _fAngleRad : Input angle
///
/// \return A valid angle between 0 and 2PI
///
////////////////////////////////////////////////////////////
float GetValidRadianAngle( float _fAngleRad );

#define ZERO_EPSILON 0.001f
// Float is zero by ZERO_EPSILON
bool IsZeroByEpsilon( float _f );

// Float is equal by ZERO_EPSILON
bool IsEqualByEpsilon( float _f1, float _f2 );

// Length of a vector
template <typename T>
T Length( const sf::Vector2<T>& _v );

template <typename T>
T SquaredLength( const sf::Vector2<T>& _v );

template <typename T>
T Length( const sf::Vector3<T>& _v );

template <typename T>
T SquaredLength( const sf::Vector3<T>& _v );

// Zero Vector
template <typename T>
void Zero( sf::Vector2<T>& _v );

template <typename T>
void Zero( sf::Vector3<T>& _v );

// Is Zero ?
template <typename T>
bool IsZero( const sf::Vector2<T>& _v );

template <typename T>
bool IsZero( const sf::Vector3<T>& _v );

// Normalize a vector
template <typename T>
void Normalize( sf::Vector2<T>& _v, float _fNewLength = 1.0f );
template <typename T>
void NormalizedCopy( const sf::Vector2<T>& _v, sf::Vector2<T>& _vOut, float _fNewLength = 1.0f );
template <typename T>
sf::Vector2<T> NormalizedCopy( const sf::Vector2<T>& _v, float _fNewLength = 1.0f );

template <typename T>
void Normalize( sf::Vector3<T>& _v, float _fNewLength = 1.0f );
template <typename T>
void NormalizedCopy( const sf::Vector3<T>& _v, sf::Vector3<T>& _vOut, float _fNewLength = 1.0f );
template <typename T>
sf::Vector3<T> NormalizedCopy( const sf::Vector3<T>& _v, float _fNewLength = 1.0f );

// Truncate a vector
template <typename T>
void Truncate( sf::Vector2<T>& _v, float _fMaxLength );
template <typename T>
void TruncateCopy( const sf::Vector2<T>& _v, const sf::Vector2<T>& _vOut, float _fMaxLength );
template <typename T>
sf::Vector2<T> TruncateCopy( sf::Vector2<T>& _v, float _fMaxLength );

template <typename T>
void Truncate( sf::Vector3<T>& _v, float _fMaxLength );
template <typename T>
void TruncateCopy( const sf::Vector3<T>& _v, const sf::Vector3<T>& _vOut, float _fMaxLength );
template <typename T>
sf::Vector3<T> TruncateCopy( sf::Vector3<T>& _v, float _fMaxLength );

// Dot Product
template <typename T>
T Dot( const sf::Vector2<T>& _v1, const sf::Vector2<T>& _v2 );

template <typename T>
T Dot( const sf::Vector3<T>& _v1, const sf::Vector3<T>& _v2 );

// Cross Product
template <typename T>
void Perp( sf::Vector2<T>& _v );

template <typename T>
sf::Vector2<T> PerpCopy( const sf::Vector2<T>& _v );

template <typename T>
void PerpCopy( const sf::Vector2<T>& _v, sf::Vector2<T>& _vOut );

template <typename T>
float Cross( const sf::Vector2<T>& _v1, const sf::Vector2<T>& _v2 );

template <typename T>
void Cross( const sf::Vector3<T>& _v1, const sf::Vector3<T>& _v2, sf::Vector3<T>& _vOut );

template <typename T>
sf::Vector3<T> Cross( const sf::Vector3<T>& _v1, const sf::Vector3<T>& _v2 );

// Transform a point from world space to an agent's local space
sf::Vector2f PointToLocalSpace( const sf::Vector2f& _Point, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide,
                                const sf::Vector2f& _AgentPosition );

// Transform a vector from world space to an agent's local space
sf::Vector2f VectorToLocalSpace( const sf::Vector2f& _Vector, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide );

// Transform a point from an agent's local space into world space
sf::Vector2f PointToWorldSpace( const sf::Vector2f& _Point, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide,
                                const sf::Vector2f& _AgentPosition );

// Transform a vector from an agent's local space into world space
sf::Vector2f VectorToWorldSpace( const sf::Vector2f& _Vector, 
                                const sf::Vector2f& _AgentHeading,
                                const sf::Vector2f& _AgentSide );

// Return positive if _v2 is clockwise to _v1, negative if anticlockwise
// Y is pointing down and X pointing right
template <typename T>
int Sign( const sf::Vector2<T>& _v1, const sf::Vector2<T>& _v2 );

#include "Math.inl"

    } // namespace math
} // namespace smeg

#endif // MATH_HPP
